#ifndef DRV_MOTOR_GO_M8010_6_H
#define DRV_MOTOR_GO_M8010_6_H

#ifdef __cplusplus
extern "C"
{
#endif

#include "main.h"
#include "motor_msg_GO_M8010_6.h"
#include "stdint.h"

#include "trajectory.h"

    typedef enum
    {
        A1,    // 4.8M baudrate
        B1_16, // 6.0M baudrate
        GO_M8010_6
    } MotorType;

    typedef enum
    {
        BRAKE,
        FOC,
        CALIBRATE,
        OPENCIRCLE = 5
    } MotorMode;

#pragma pack(1)
    /*******************************************************************
     * @brief  电机期望值设定，用于设定电机的期望值 des = desired value
     * @param mode,ID,tor_des,spd_des,pos_des,kp,k_spd
     ******************************************************************/
    typedef struct MotorSetValTypeDef
    {
        MotorType motorType; // 电机类型
        uint8_t ID;          // 电机编号，可以为 0、1、2、0xBB，0xBB代表向所有电机广播
        MotorMode mode;      // 运行模式
        float tor_des;       // 期望关节输出扭矩 unit: N.m     (q8)
        float spd_des;       // 期望关节输出速度 unit: rad/s   (q7)
        float pos_des;       // 期望关节输出位置 unit: rad     (q15)
        float kp_des;        // 期望关节刚度系数 unit: 0.0-1.0 (q15)
        float kd_des;        // 期望关节阻尼系数 unit: 0.0-1.0 (q15)
        uint8_t hex_len;     // 发送数据长度
        float motionCurve;   // 运动曲线
    } MotorSetValTypeDef;
#pragma pack()

#pragma pack(1)
    typedef struct MotorGetValTypeDef
    {
        MotorType motorType;
        int hex_len;
        unsigned char motor_id;
        unsigned char mode;
        int temp;
        int merror;
        float tau;
        float dq;
        float q;

        int correct;

        int footForce;
        float LW;
        int acc;

        float gyro[3];
        float accBd[3];
    } MotorGetValTypeDef;
#pragma pack()

#pragma pack(1)
    typedef struct Motor_COM_t
    {
        GO_M8010_6_ControlData motor_cmd_msg;
        GO_M8010_6_MotorData motor_fdk_msg;

        uint8_t RS485_IDLE_FLAG;

        UART_HandleTypeDef *motor_huart;

        uint8_t getFDKFlag;
        uint8_t TimeOutFlag;

#ifdef FREERTOS
        SemaphoreHandle_t xArmMotorMsgSendSemaphore;
#endif

    } Motor_COM_t;

#pragma pack()

#pragma pack(1)
    typedef struct Device_Motor_t
    {
        MotorSetValTypeDef motor_cmd;
        MotorGetValTypeDef motor_state;

        Trajectory trajectoryPlanner;
        UART_HandleTypeDef *huart;
        uint32_t packetLossCount;

        // GO_M8010_6_ControlData motor_cmd_msg;
        // GO_M8010_6_MotorData motor_fdk_msg;

    } Device_Motor_t;
#pragma pack()

    void Motor_GO_M8010_6_Init(Motor_COM_t *motor_com, Device_Motor_t *dev_motor_t, UART_HandleTypeDef *motor_huart, uint8_t Motor_ID);
    int unpackMsg2GetVal(const GO_M8010_6_MotorData *motorMsg, MotorGetValTypeDef *getVal);
    int packSetVal2Msg(GO_M8010_6_ControlData *cmdMsg, MotorSetValTypeDef *setVal);

#ifdef FREERTOS
    HAL_StatusTypeDef Motor_GO_M8010_Send_Receive(Device_Motor_t *dev_motor_t, Motor_COM_t *motor_com, SemaphoreHandle_t xMotor485IDLESemaphore);
    void Motor_GO_M8010_SetZeroPosition(Device_Motor_t *dev_motor_t, Motor_COM_t *motor_com, SemaphoreHandle_t xMotor485IDLESemaphore);
#else
HAL_StatusTypeDef Motor_GO_M8010_Send_Receive(Device_Motor_t *dev_motor_t, Motor_COM_t *motor_com);
#endif

#ifdef __cplusplus
}
#endif /* extern "C */

#endif /* DRV_MOTOR_GO_M8010_6_H */
